#include "PointCloudFilterTable.h"

PointCloudFilterTable::PointCloudFilterTable(double _sigma, double _sigma2, double _angleT, bool _horizontalAssumption) {
  minNrRANSACIterations = 50;
  maxNrRANSACIterations = 150;
  minSamples = 3;
  sigma = _sigma; // 15, Texlets further from the table are excluded
  sigma2 = _sigma2; // 8, Should be smaller than sigma. Above this threshold, texlets with a different 
                     // normal orientation than the plane estimation are excludes
  angleT = _angleT; // Angle threshold between texlet orientation and table plane orientation
  R1 = 100;   // All texlets closer are not considered
  R2 = 800;  // All texlets further are not considered
  R1 = R1*R1;
  R2 = R2*R2;
  horizontalAssumption = _horizontalAssumption;
  planeEquation.estimated = false;
}

PointCloudFilterTable::~PointCloudFilterTable() {
}

void PointCloudFilterTable::setParameters(double _sigma, double _sigma2, double _angleT, bool _horizontalAssumption) {
  sigma = _sigma; // 15, Texlets further from the table are excluded
  sigma2 = _sigma2; // 8, Should be smaller than sigma. Above this threshold, texlets with a different 
                     // normal orientation than the plane estimation are excludes
  angleT = _angleT; // Angle threshold between texlet orientation and table plane orientation
  horizontalAssumption = _horizontalAssumption;
}

void PointCloudFilterTable::fitTablePlane(vector<Texlet*> &texlets) {
  //vector<Texlet*> inputSet;
  //for(size_t i=0; i<texlets.size(); i++) 
  //  inputSet.push_back( texlets[i] );

  TablePlane tablePlaneFitting(minNrRANSACIterations, maxNrRANSACIterations, sigma, minSamples, sigma2, angleT, horizontalAssumption);
  tablePlaneFitting.setInputSet(texlets);
  tablePlaneFitting.fitModel();

  tablePlaneFitting.getConsensus(texletsTable);


  vector<double> tablePlaneVector = tablePlaneFitting.getTablePlaneVector();
  planeEquation.a = tablePlaneVector[0];
  planeEquation.b = tablePlaneVector[1];
  planeEquation.c = tablePlaneVector[2];
  planeEquation.d = tablePlaneVector[3];
  planeEquation.nABC = sqrt(planeEquation.a*planeEquation.a + planeEquation.b*planeEquation.b + planeEquation.c*planeEquation.c);
  planeEquation.estimated = true;

  cout << "Plane: " << planeEquation.a << ", " << planeEquation.b << ", " << 
    planeEquation.c << ", " << planeEquation.d << endl;

}

vector<Texlet*> PointCloudFilterTable::filter(vector<Texlet*> &texletsIn) {
  if(!planeEquation.estimated)
    fitTablePlane(texletsIn);

  vector<Texlet*> inliers;

  if(planeEquation.c >= 0) { // Normal points downwards, away from the camera, D>0 is above the table
    for(size_t i=0; i<texletsIn.size(); i++) {
      //      if(includeUpward(texletsIn[i].getPosition()[0], texletsIn[i].getPosition()[1], texletsIn[i].getPosition()[2],
      //		 texletsIn[i].getNormalVectorPlanePatch()[0], texletsIn[i].getNormalVectorPlanePatch()[1], texletsIn[i].getNormalVectorPlanePatch()[2]))
      if(includeUpward(texletsIn[i]->pos[0], texletsIn[i]->pos[1], texletsIn[i]->pos[2],
			 texletsIn[i]->ori[0], texletsIn[i]->ori[1], texletsIn[i]->ori[2]))
	inliers.push_back( texletsIn[i] );
    }
  }
  else { // c<0, Normal points upwards, towards the camera, D<0 is above the table
    for(size_t i=0; i<texletsIn.size(); i++) {
      if(includeDownward(texletsIn[i]->pos[0], texletsIn[i]->pos[1], texletsIn[i]->pos[2],
			 texletsIn[i]->ori[0], texletsIn[i]->ori[1], texletsIn[i]->ori[2]))
	inliers.push_back( texletsIn[i] );
    }
  }
  return inliers; 
}


InOut* PointCloudFilterTable::filterInOut(vector<Texlet*> &texletsIn) {
  if(!planeEquation.estimated)
    fitTablePlane(texletsIn);

  //vector<texlet3D *> * inliers = tablePlaneFitting.getBestConsensusSet();
  //vector<texlet3D *> * outliers = tablePlaneFitting.getBestOutliersSet();

  InOut* inOut = new InOut;
  //inOut.inliers = inliers;
  //inOut.outliers = outliers;

  //vector<Texlet *> * inliers = new vector<Texlet *>;
  //vector<Texlet *> * outliers = new vector<Texlet *>;

  if(planeEquation.c >= 0) { // Normal points downwards, away from the camera, D>0 is above the table
    for(size_t i=0; i<texletsIn.size(); i++) {
      if(includeUpward(texletsIn[i]->pos[0], texletsIn[i]->pos[1], texletsIn[i]->pos[2],
		       texletsIn[i]->ori[0], texletsIn[i]->ori[1], texletsIn[i]->ori[2]))
	inOut->inliers.push_back( texletsIn[i] );
      else
	inOut->outliers.push_back( texletsIn[i] );
    }
  }
  else { // c<0, Normal points upwards, towards the camera, D<0 is above the table
    for(size_t i=0; i<texletsIn.size(); i++) {
      if(includeDownward(texletsIn[i]->pos[0], texletsIn[i]->pos[1], texletsIn[i]->pos[2],
			 texletsIn[i]->ori[0], texletsIn[i]->ori[1], texletsIn[i]->ori[2]))
	inOut->inliers.push_back( texletsIn[i] );
      else
	inOut->outliers.push_back( texletsIn[i] );
    }
  }
  return inOut; //TODO: delete inliers and outliers when done
}


/*!
 * Should this point be included. This implements the case where the normal of the table plane 
 * points towards the camera
 */
inline bool PointCloudFilterTable::includeUpward(double x, double y, double z, double n1, double n2, double n3) {
  double D,R,A;
  double a = planeEquation.a;
  double b = planeEquation.b;
  double c = planeEquation.c;
  double d = planeEquation.d;
  double nABC = planeEquation.nABC;
  R = x*x + y*y + z*z;
  if(R>=R1 && R<=R2) {
    D = (x*a + y*b + z*c + d) / nABC;
    if(D>sigma)
      return true;
    else if(D>sigma2) {
      A = acos( (n1*a + n2*b + n3*c) / nABC );
      if(A>angleT)
	return true;
      else
	return false;
    }
    else
      return false;
  }
  else
    return false;
}

/*!
 * Should this point be included. This implements the case where the normal of the table plane 
 * points away from the camera
 */
inline bool PointCloudFilterTable::includeDownward(double x, double y, double z, double n1, double n2, double n3) {
  double D,R,A;
  double a = planeEquation.a;
  double b = planeEquation.b;
  double c = planeEquation.c;
  double d = planeEquation.d;
  double nABC = planeEquation.nABC;
  R = x*x + y*y + z*z;
  if(R>=R1 && R<=R2) {
    D = (x*a + y*b + z*c + d) / nABC;
    if(D<-sigma)
      return true;
    else if(D<-sigma2) {
      A = acos( (n1*a + n2*b + n3*c) / nABC );
      if(A>angleT)
	return true;
      else
	return false;
    }
    else
      return false;
  }
  else
    return false;
}

/*!
 * Should this point be included. This implements the case where the normal of the table plane 
 * points towards the camera
 */
inline bool PointCloudFilterTable::includeUpward(double x, double y, double z) {
  double D,R;
  double a = planeEquation.a;
  double b = planeEquation.b;
  double c = planeEquation.c;
  double d = planeEquation.d;
  double nABC = planeEquation.nABC;
  R = x*x + y*y + z*z;
  if(R>=R1 && R<=R2) {
    D = (x*a + y*b + z*c + d) / nABC;
    if(D>sigma)
      return true;
    else
      return false;
  }
  else
    return false;
}

/*!
 * Should this point be included. This implements the case where the normal of the table plane 
 * points away from the camera
 */
inline bool PointCloudFilterTable::includeDownward(double x, double y, double z) {
  double D,R;
  double a = planeEquation.a;
  double b = planeEquation.b;
  double c = planeEquation.c;
  double d = planeEquation.d;
  double nABC = planeEquation.nABC;
  R = x*x + y*y + z*z;
  if(R>=R1 && R<=R2) {
    D = (x*a + y*b + z*c + d) / nABC;
    if(D<-sigma)
      return true;
    else
      return false;
  }
  else
    return false;
}
